/*
 * @file trunk_handler.cpp
 *
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @brief	emotion handler of the trunk.
 */

#include "emotion/trunk_handler.h"

// constructor
TrunkHandler::TrunkHandler()
{
	_trunk_pub = nh_.advertise<comm::trunk>(Topic::MOTOR_TRUNK, 1);
}

// destructor
TrunkHandler::~TrunkHandler()
{

}

void TrunkHandler::init() {
}

void TrunkHandler::spin() {
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		if (emoteOk()) {
			processEmotion();
			publish();
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Publish trunk emotion.
 */
void TrunkHandler::publish()
{
	_trunk_pub.publish(_trunk_state);
}

/**
 * Set the state of the trunk according to the emotion.
 */
void TrunkHandler::processEmotion() {
	switch (getEmotion()) {
	case EMOTION_BORED:
		setTrunkState(TRUNK_DOWN);
		break;
	case EMOTION_SAD:
		setTrunkState(TRUNK_DOWN);
		break;
	default:
		setTrunkState(TRUNK_NEUTRAL);
		setTrunkAngle(0.25);
		break;
	}
}

/**
 * Set the trunk state.
 * @state	state of the trunk (emotion state).
 */
void TrunkHandler::setTrunkState(int state)
{
	_trunk_state.state = state;
}

/**
 * Set the trunk angle.
 * @angle	angle of the trunk (used for rviz only).
 */
void TrunkHandler::setTrunkAngle(float angle)
{
	_trunk_state.angle = angle;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "trunkHandlerEmotion");

	TrunkHandler handler;
	handler.init();
	handler.spin();
}


